import json
import heapq
import requests

def load(filename):
    with open(filename,'r',encoding='UTF-8') as file:
        data = json.load(file)
    return data

info = r'Car-Robot\data\scene_info.json'
map_location = r'Car-Robot\data\map_location.json'
navi_point = r'Car-Robot\data\navi_point.json'
path = r'Car-Robot\data\path.json'
robot =r'Car-Robot\data\robot_loc.json'

class MapFunction:



    def __init__(self):
        self.navi_point_json = load(navi_point)
        self.path_json = load(path)
        self.map_location_json = load(map_location)
        self.point_dic = {}
        
        for location in self.map_location_json["locationList"]:
            self.point_dic[location["location_id"]] = (location["pos_x"],location["pos_y"])
        for point in self.navi_point_json["pointList"]:
            self.point_dic[point["point_id"]] = (point["pos_x"],point["pos_y"])
        self.graph = self.graph_gen()
    def cal_dis(self,src_id,dst_id):
        '''计算src和dst之间的距离'''
        src_point = self.point_dic[src_id]
        dst_point = self.point_dic[dst_id]
        dis = ((src_point[0]-dst_point[0])**2 + (src_point[1]-dst_point[1])**2)**0.5

        return dis
    
    def graph_gen(self):

        graph = {}
        for point in self.point_dic:
            # self.point_dic[point["point_id"]] = (point["pos_x"],point["pos_y"])
            graph[point] = {}
        for path in self.path_json["pathList"]:
            graph[path["src_id"]] = {}
            graph[path["dst_id"]] = {}
        for path in self.path_json["pathList"]:
            graph[path["src_id"]][path["dst_id"]] = self.cal_dis(path["src_id"],path["dst_id"])
            graph[path["dst_id"]][path["src_id"]] = self.cal_dis(path["src_id"],path["dst_id"])


        return graph
    
    def dijkstra_shortest_path(self,start, end, graph):
        predecessors = {}
        # 初始化距离字典
        distances = {node: float('infinity') for node in graph}
        distances[start] = 0
    
        # 初始化优先队列
        pq = [(0, start)]
    
        while pq:
            current_distance, current_node = heapq.heappop(pq)
            
            if current_node == end:
                path = []
                while current_node in predecessors:
                    path.insert(0, current_node)
                    current_node = predecessors[current_node]
                path.insert(0, start)
                return path
            
            if current_distance > distances[current_node]:
                continue
            
            for neighbor, weight in graph[current_node].items():
                distance = current_distance + weight
                if distance < distances[neighbor]:
                    distances[neighbor] = distance
                    heapq.heappush(pq, (distance, neighbor))
                    predecessors[neighbor] = current_node
        
        return None
    
    def robot_location(self,filename):
        self.robot_json = load(filename)
        robot_loc = []
        for robot in self.robot_json["robotList"]:

            robot_pos = (robot["posX"],robot["posY"])
            for point in self.point_dic:
                if self.point_dic[point] == robot_pos:

                    robot_loc.append(point)
        
        return robot_loc

    def multi_dikstra_shortest_path(self,robot_loc, dst_loc,graph):
        path_dic = {}
        for i in range(len(robot_loc)):
            path = self.dijkstra_shortest_path(robot_loc[i], dst_loc[i], self.graph_gen())
            path_dic[robot_loc[i]] = path
        return path_dic
    
    def car_command_gen(self,path_dic):
        start_robot = 106
        command_list = []
        for i in range(5):
            payload = {
                "robotId": start_robot+i,
                "command": "chas-backward {0} {1} 1.0 1".format(self.point_dic[path_dic[i][0]][0],self.point_dic[path_dic[i][0]][1]),
            }
            command_list.append(payload)
        return command_list
    
    def robot_loc_by_dic(self,loc):
        
        robot_loc = []
        robot_id = 106
        for i in range(5):
            for point in self.point_dic:
                if self.point_dic[point] == loc[robot_id + i]:
                    
                    robot_loc.append(point)
                
        
        return robot_loc
            